#include "../User/Task/IMU.h"

#include "../User/Device/IMU.h"
#include "cmsis_os.h"
#include "main.h"
#include "sys.h"

Task_Runtime_t* t;

/* USER CODE BEGIN Header_Task_IMU */
/**
 * @brief Function implementing the TaskIMU thread.
 * @param argument: Not used
 * @retval None
 */
/* USER CODE END Header_Task_IMU */
void Task_IMU(void* argument) {
  const uint32_t delay_tick = osKernelGetTickFreq() / TASK_IMU_SCAN_FREQ;

  IMU_Init();

  uint32_t tick = osKernelGetTickCount();
	
	t = &Task_Runtime;

  while (1) {
    tick += delay_tick;

    IMU_GetData(Task_Runtime.AccData, Task_Runtime.GyroData);

    IMU_PraseData(&(Task_Runtime.accel), &(Task_Runtime.gyro),
                  Task_Runtime.AccData, Task_Runtime.GyroData,
                  &(Task_Runtime.angle));

    osDelayUntil(tick);
  }
}
